using BepuUtilities;
using BepuUtilities.Memory;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using System.Threading;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics
{
    public interface IPoseIntegrator
    {
        void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null);
        void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null);
        void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null);
        void IntegrateVelocitiesAndUpdateInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null);
        void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null);
    }

    /// <summary>
    /// 定义姿势积分器应如何处理角速度积分。
    /// </summary>
    public enum AngularIntegrationMode
    {
        /// <summary>
        /// 角速度是直接积分的,不会随着身体姿势的改变而改变。不会保持角动量。
        /// </summary>
        Nonconserving,
        /// <summary>
        /// 通过根据方向的变化更新角速度,近似守恒角动量。对于陀螺仪来说,这是一项不错的工作,但角速度往往会朝着最小的惯性轴漂移。
        /// </summary>
        ConserveMomentum,
        /// <summary>
        /// 通过包含隐式陀螺力矩,近似守恒角动量。Dzhanibekov效应模拟的最佳选择,但应用的阻尼效果可能会降低陀螺仪的用途。
        /// </summary>
        ConserveMomentumWithGyroscopicTorque,
    }

    /// <summary>
    /// 定义处理身体姿势集成的回调的类型。
    /// </summary>
    public interface IPoseIntegratorCallbacks
    {
        /// <summary>
        /// 获取姿势积分器应如何处理角速度积分。
        /// </summary>
        AngularIntegrationMode AngularIntegrationMode { get; }

        /// <summary>
        /// 在集成模拟的活动实体之前调用。当与子步进计时器一起使用时,可以使用不同的时间步长值在每帧中多次调用此函数。
        /// </summary>
        /// <param name="dt">当前时间步持续时间。</param>
        void PrepareForIntegration(float dt);

        /// <summary>
        /// 在实体集成期间,为模拟中的每个活动实体调用回调。
        /// </summary>
        /// <param name="bodyIndex">要访问的正文的索引。</param>
        /// <param name="pose">正文的当前姿势。</param>
        /// <param name="localInertia">身体当前的局部惯性。</param>
        /// <param name="workerIndex">处理此正文的工作线程的索引。</param>
        /// <param name="ocity">对要积分的物体当前速度的引用。</param>
        void IntegrateVelocity(int bodyIndex, in RigidPose pose, in BodyInertia localInertia, int workerIndex, ref BodyVelocity velocity);
    }

    /// <summary>
    /// 提供用于集成身体姿势的辅助函数。
    /// </summary>
    public static class PoseIntegration
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void RotateInverseInertia(in Symmetric3x3 localInverseInertiaTensor, in Quaternion orientation, out Symmetric3x3 rotatedInverseInertiaTensor)
        {
            Matrix3x3.CreateFromQuaternion(orientation, out var orientationMatrix);
            // I^-1=RT*Ilocal^-1*R
            // 注意：如果您愿意稍微混淆一下用户,可以要求局部惯性为对角线。
            // 这对于碰巧具有对角惯性的所有基本体类型都是完全合适的,但是对于更复杂的形状(凸面外壳、网格),
            // 这将需要一个重新定位的步骤。这可能会令人困惑,而且可能不值得这样做。
            Symmetric3x3.RotationSandwich(orientationMatrix, localInverseInertiaTensor, out rotatedInverseInertiaTensor);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Integrate(in Vector3 position, in Vector3 linearVelocity, float dt, out Vector3 integratedPosition)
        {
            position.Validate();
            linearVelocity.Validate();
            var displacement = linearVelocity * dt;
            integratedPosition = position + displacement;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe static void Integrate(in Quaternion orientation, in Vector3 angularVelocity, float dt, out Quaternion integratedOrientation)
        {
            orientation.ValidateOrientation();
            angularVelocity.Validate();
            // 请注意,我们不考虑角动量守恒,或陀螺仪项,或其他任何东西。所有方向积分假定为一系列分段线性积分
            // 这并不完全正确,但这是一个合理的近似,这意味着我们在处理CCD扫描时,不必担心角动量守恒或陀螺项。
            var speed = angularVelocity.Length();
            if (speed > 1e-15f)
            {
                var halfAngle = speed * dt * 0.5f;
                Quaternion q;
                Unsafe.As<Quaternion, Vector3>(ref *&q) = angularVelocity * (MathHelper.Sin(halfAngle) / speed);
                q.W = MathHelper.Cos(halfAngle);
                // 请注意,输入和输出可能会重叠。
                QuaternionEx.Concatenate(orientation, q, out integratedOrientation);
                QuaternionEx.Normalize(ref integratedOrientation);
            }
            else
            {
                integratedOrientation = orientation;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Integrate(in QuaternionWide start, in Vector3Wide angularVelocity, in Vector<float> halfDt, out QuaternionWide integrated)
        {
            start.Validate();
            angularVelocity.Validate();
            Vector3Wide.Length(angularVelocity, out var speed);
            var halfAngle = speed * halfDt;
            QuaternionWide q;
            MathHelper.Sin(halfAngle, out var s);
            var scale = s / speed;
            q.X = angularVelocity.X * scale;
            q.Y = angularVelocity.Y * scale;
            q.Z = angularVelocity.Z * scale;
            MathHelper.Cos(halfAngle, out q.W);
            QuaternionWide.ConcatenateWithoutOverlap(start, q, out var concatenated);
            QuaternionWide.Normalize(concatenated, out integrated);
            var speedValid = Vector.GreaterThan(speed, new Vector<float>(1e-15f));
            integrated.X = Vector.ConditionalSelect(speedValid, integrated.X, start.X);
            integrated.Y = Vector.ConditionalSelect(speedValid, integrated.Y, start.Y);
            integrated.Z = Vector.ConditionalSelect(speedValid, integrated.Z, start.Z);
            integrated.W = Vector.ConditionalSelect(speedValid, integrated.W, start.W);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe void Integrate(in RigidPose pose, in BodyVelocity velocity, float dt, out RigidPose integratedPose)
        {
            Integrate(pose.Position, velocity.Linear, dt, out integratedPose.Position);
            Integrate(pose.Orientation, velocity.Angular, dt, out integratedPose.Orientation);
        }
    }


    /// <summary>
    /// 将移动体随时间的速度集成到位置和方向的更改中。也会将重力加速度应用于动力学实体。
    /// </summary>
    /// <remarks>
    /// 积分器的此变体使用单个全局重力。其他提供每实体重力的积分器稍后可能会存在。
    /// 该积分器还假设以单精度浮点数存储主体位置。稍后,我们可能会修改身体
    /// 存储,以允许对较大的模拟使用不同的表示法。这将需要改变这个积分器,碰撞检测的相对位置计算,
    /// 边界框计算,在极端情况下甚至可能处于宽相位(每个分量位置64位)。
    /// </remarks>
    public class PoseIntegrator<TCallbacks> : IPoseIntegrator where TCallbacks : IPoseIntegratorCallbacks
    {
        Bodies bodies;
        Shapes shapes;
        BroadPhase broadPhase;

        TCallbacks callbacks;

        Action<int> integrateBodiesAndUpdateBoundingBoxesWorker;
        Action<int> predictBoundingBoxesWorker;
        Action<int> integrateVelocitiesBoundsAndInertiasWorker;
        Action<int> integrateVelocitiesWorker;
        Action<int> integratePosesWorker;
        public PoseIntegrator(Bodies bodies, Shapes shapes, BroadPhase broadPhase, TCallbacks callback)
        {
            this.bodies = bodies;
            this.shapes = shapes;
            this.broadPhase = broadPhase;
            this.callbacks = callback;
            integrateBodiesAndUpdateBoundingBoxesWorker = IntegrateBodiesAndUpdateBoundingBoxesWorker;
            predictBoundingBoxesWorker = PredictBoundingBoxesWorker;
            integrateVelocitiesBoundsAndInertiasWorker = IntegrateVelocitiesBoundsAndInertiasWorker;
            integrateVelocitiesWorker = IntegrateVelocitiesWorker;
            integratePosesWorker = IntegratePosesWorker;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void UpdateSleepCandidacy(ref BodyVelocity velocity, ref BodyActivity activity)
        {
            var velocityHeuristic = velocity.Linear.LengthSquared() + velocity.Angular.LengthSquared();
            if (velocityHeuristic > activity.SleepThreshold)
            {
                activity.TimestepsUnderThresholdCount = 0;
                activity.SleepCandidate = false;
            }
            else
            {
                ++activity.TimestepsUnderThresholdCount;
                if (activity.TimestepsUnderThresholdCount >= activity.MinimumTimestepsUnderThreshold)
                {
                    activity.SleepCandidate = true;
                }
            }

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void IntegrateAngularVelocityConserving(in Quaternion previousOrientation, in RigidPose pose, in BodyInertia localInertia, in BodyInertia inertia, ref Vector3 angularVelocity, float dt)
        {
            previousOrientation.ValidateOrientation();
            pose.Orientation.ValidateOrientation();
            angularVelocity.Validate();

            if (callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentum)
            {
                // 请注意,这实际上会重新计算前一帧的惯性。惯性缓冲器中可能没有存储以前的惯性。
                // 这就避免了使用添加/删除/移动和其他我们不容易跟踪的状态更改来保持世界惯性缓冲区更新的相当复杂的需要。
                // 此外,即使缓存了它,加载另一个惯性张量的内存带宽要求也会对多线程伸缩造成足够的影响,从而消除任何性能优势。
                Matrix3x3.CreateFromQuaternion(previousOrientation, out var previousOrientationMatrix);
                Matrix3x3.TransformTranspose(angularVelocity, previousOrientationMatrix, out var localPreviousAngularVelocity);
                Symmetric3x3.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor);
                Symmetric3x3.TransformWithoutOverlap(localPreviousAngularVelocity, localInertiaTensor, out var localAngularMomentum);
                Matrix3x3.Transform(localAngularMomentum, previousOrientationMatrix, out var angularMomentum);
                Symmetric3x3.TransformWithoutOverlap(angularMomentum, inertia.InverseInertiaTensor, out angularVelocity);
            }

            // 请注意,此模式分支针对任何返回常量值的回调进行了优化。
            if (callbacks.AngularIntegrationMode == AngularIntegrationMode.ConserveMomentumWithGyroscopicTorque)
            {
                // 显式积分陀螺力可能会导致一些不稳定,因此我们将使用近似隐式方法。
                // angularVelocity1*惯性1=angularVelocity0*惯性1+dt*((angularVelocity1*惯性1)x angularVelocity1)
                // 请注意,这包括对inetia1的引用,但该引用尚不存在。然而,我们确实有局部惯性,所以我们将
                // 使用计算的当前方向将所有速度转换到局部空间。
                // 因此：
                // localAngularVelocity1*localInertia=localAngularVelocity0*localInertia-dt*(localAngularVelocity1 x(localAngularVelocity1*localInertia))
                // localAngularVelocity1*localInertia-localAngularVelocity0*localInertia+dt*(localAngularVelocity1 x(localAngularVelocity1*localInertia))=0
                // F(LocalAngularVelocity1)=(localAngularVelocity1-localAngularVelocity0)*localInertia+dt*(localAngularVelocity1 x(localAngularVelocity1*localInertia))
                // 求解localAngularVelocity1并不容易,所以我们将使用牛顿迭代进行数值求解。
                // (对于熟悉Bullet的BT_ENABLE_GYROCOMIC_FORCE_IMPLICIT_BODY的读者来说,这基本上是相同的。)

                // 我们将从初始猜测localAngularVelocity1=localAngularVelocity0开始,并使用牛顿步长f(LocalAngularVelocity1)*Invert(df/DW1(LocalAngularVelocity1))进行更新
                // df/dw1x(LocalAngularVelocity1)*localInertia+dt*(df/dw1x(LocalAngularVelocity1)x(localAngularVelocity1*localInertia)+localAngularVelocity1 x df/dw1x(localAngularVelocity1*localInertia))
                // df/dw1x(LocalAngularVelocity1)=(1,0,0)
                // df/dw1x(f(LocalAngularVelocity1))=(1,0,0)*localInertia+dt*((1,0,0)x(localAngularVelocity1*localInertia)+localAngularVelocity1 x((1,0,0)*localInertia))
                // df/dw1x(f(LocalAngularVelocity1))=(0,1,0)*localInertia+dt*((0,1,0)x(localAngularVelocity1*localInertia)+localAngularVelocity1 x((0,1,0)*localInertia))
                // df/dw1x(f(LocalAngularVelocity1))=(0,0,1)*localInertia+dt*((0,0,1)x(localAngularVelocity1*localInertia)+localAngularVelocity1 x((0,0,1)*localInertia))
                // 在给定a x b=skew(A)*b的情况下,这可以更简洁地表示,其中skew(A)是表示叉积的倾斜对称矩阵：
                // df/DW1(f(LocalAngularVelocity1))=localInertia+dt*(skew(LocalAngularVelocity1)*localInertia-skew(localAngularVelocity1*localInertia))
                Matrix3x3.CreateFromQuaternion(pose.Orientation, out var orientationMatrix);
                // 使用localAngularVelocity0作为localAngularVelocity1的第一个猜测。
                Matrix3x3.TransformTranspose(angularVelocity, orientationMatrix, out var localAngularVelocity);
                Symmetric3x3.Invert(localInertia.InverseInertiaTensor, out var localInertiaTensor);

                Symmetric3x3.TransformWithoutOverlap(localAngularVelocity, localInertiaTensor, out var localAngularMomentum);
                var residual = dt * Vector3.Cross(localAngularMomentum, localAngularVelocity);

                Matrix3x3.CreateCrossProduct(localAngularMomentum, out var skewMomentum);
                Matrix3x3.CreateCrossProduct(localAngularVelocity, out var skewVelocity);
                Symmetric3x3.Multiply(skewVelocity, localInertiaTensor, out var transformedSkewVelocity);
                Matrix3x3.Subtract(transformedSkewVelocity, skewMomentum, out var changeOverDt);
                Matrix3x3.Scale(changeOverDt, dt, out var change);
                Symmetric3x3.Add(change, localInertiaTensor, out var jacobian);

                Matrix3x3.Invert(jacobian, out var inverseJacobian);
                Matrix3x3.Transform(residual, inverseJacobian, out var newtonStep);
                localAngularVelocity -= newtonStep;

                Matrix3x3.Transform(localAngularVelocity, orientationMatrix, out angularVelocity);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void IntegrateAngularVelocity(in Quaternion previousOrientation, in RigidPose pose, in BodyInertia localInertia, in BodyInertia inertia, ref Vector3 angularVelocity, float dt)
        {
            // 请注意,此模式分支针对任何返回常量值的回调进行了优化。
            if ((int)callbacks.AngularIntegrationMode >= (int)AngularIntegrationMode.ConserveMomentum)
            {
                if (!Bodies.HasLockedInertia(localInertia.InverseInertiaTensor))
                {
                    IntegrateAngularVelocityConserving(previousOrientation, pose, localInertia, inertia, ref angularVelocity, dt);
                }
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void IntegrateAngularVelocity(in RigidPose pose, in BodyInertia localInertia, in BodyInertia inertia, ref Vector3 angularVelocity, float dt)
        {
            // 我们没有以前的入职培训。通过向后集成来重建它。
            // (就单线程而言,缓存此信息可能会更快,但它会增加大量复杂性,并可能最终降低内核数量较高时的性能。)
            // 请注意,此模式分支针对任何返回常量值的回调进行了优化。
            if ((int)callbacks.AngularIntegrationMode >= (int)AngularIntegrationMode.ConserveMomentum)
            {
                if (!Bodies.HasLockedInertia(localInertia.InverseInertiaTensor))
                {
                    PoseIntegration.Integrate(pose.Orientation, angularVelocity, -dt, out var previousOrientation);
                    IntegrateAngularVelocityConserving(previousOrientation, pose, localInertia, inertia, ref angularVelocity, dt);
                }
            }
        }

        unsafe void IntegrateBodiesAndUpdateBoundingBoxes(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex)
        {
            ref var basePoses = ref bodies.ActiveSet.Poses[0];
            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0];
            ref var baseInertias = ref bodies.Inertias[0];
            ref var baseActivity = ref bodies.ActiveSet.Activity[0];
            ref var baseCollidable = ref bodies.ActiveSet.Collidables[0];
            for (int i = startIndex; i < endIndex; ++i)
            {
                ref var pose = ref Unsafe.Add(ref basePoses, i);
                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);

                var previousOrientation = pose.Orientation; // 如果角动量守恒被禁用.编译器*可能*将其删除.
                PoseIntegration.Integrate(pose, velocity, dt, out pose);

                // 请注意,这通常在速度积分之前使用。这意味着物体可以在重力诱导的速度下变得不活跃。
                // 这实际上是为了：当窄相位唤醒一个岛屿时,岛上积累的脉冲将准备好接受重力的影响。
                // 否则,将损害解算器的猜测,降低解算的质量,并可能导致轻微的凹凸。
                // 这仅在更新顺序实际上将卧铺置于重力之后时才相关。为了便于使用,模拟更新顺序可能会忽略这一事实。
                UpdateSleepCandidacy(ref velocity, ref Unsafe.Add(ref baseActivity, i));

                // 更新新方向的惯性张量。
                // TODO：如果姿势积分器位于更新的末尾,则任何超时步长方向更改或局部惯性更改后的第一帧
                // 必须得到的是在别处计算的惯性张量。它们要么需要在加法上计算,要么是其他什么-这有点粗略,但可以做到-
                // 或者,我们需要将此计算移到帧的开头,以保证所有惯性都是最新的。
                // 这将需要扫描所有姿势内存才能支持,但如果您在更新AABB的同时执行此操作,这很好-该阶段也使用该姿势。
                ref var localInertia = ref Unsafe.Add(ref baseLocalInertia, i);
                ref var inertia = ref Unsafe.Add(ref baseInertias, i);
                PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out inertia.InverseInertiaTensor);
                // 虽然仅仅复制每一帧的反质量有点傻,即使它没有改变,
                // 它几乎总是和惯性张量聚集在一起,它真的不值得整个额外的外部系统,只按需复制逆质量。
                inertia.InverseMass = localInertia.InverseMass;

                IntegrateAngularVelocity(previousOrientation, pose, localInertia, inertia, ref velocity.Angular, dt);
                callbacks.IntegrateVelocity(i, pose, localInertia, workerIndex, ref velocity);

                // 边界框是以标量方式累积的,但是实际的边界框计算会延迟,直到累积了足够数量的可碰撞对象才能进行
                // 执行一个有价值的捆绑包。这会做两件事：
                // 1)SIMD可以用来做包围盒计算的数学比特。计算通常很便宜,
                // 但它们通常会比上面的姿势材料更贵。
                // 2)将所需的虚拟函数调用次数减少等于累加器高速缓存大小的因子。
                // 请注意,累加器缓存保持相对较小,因此可碰撞物体的姿势和速度很可能仍在L1缓存中
                // 当需要实际计算边界框时。

                // 请注意,任何缺少可碰撞的可碰撞的可碰撞对象,或任何超出可碰撞对象集合的引用,都将具有特殊形成的索引。
                // 累加器将检测到这一点,并且不会尝试添加不存在的可碰撞。
                boundingBoxBatcher.Add(i, pose, velocity, Unsafe.Add(ref baseCollidable, i));

                // 在姿势积分器中进行边界框更新很有帮助,因为它们共享信息。如果这两个阶段分开,可能会受到处罚
                // 与再次从记忆中加载所有身体姿势和速度相关。即使L3缓存持续存在,它仍然比查看L1或L2更糟糕。
                // 此外,孤立的姿势积分器对内存的限制非常大,以至于它几乎不能从多线程中获益。通过交错一些较少的内存限制
                // 在组合中工作,我们有望填补一些执行空白。
            }
        }

        unsafe void PredictBoundingBoxes(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex)
        {
            ref var basePoses = ref bodies.ActiveSet.Poses[0];
            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0];
            ref var baseActivity = ref bodies.ActiveSet.Activity[0];
            ref var baseCollidable = ref bodies.ActiveSet.Collidables[0];
            for (int i = startIndex; i < endIndex; ++i)
            {
                ref var pose = ref Unsafe.Add(ref basePoses, i);
                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
                pose.Position.Validate();
                pose.Orientation.ValidateOrientation();
                velocity.Linear.Validate();
                velocity.Angular.Validate();

                UpdateSleepCandidacy(ref velocity, ref Unsafe.Add(ref baseActivity, i));

                // 边界框预测不需要更新惯性张量。
                var integratedVelocity = velocity;
                callbacks.IntegrateVelocity(i, pose, Unsafe.Add(ref baseLocalInertia, i), workerIndex, ref integratedVelocity);

                // 请注意,我们不包括用于包围盒预测的更复杂的角度积分-这不是很重要。
                boundingBoxBatcher.Add(i, pose, integratedVelocity, Unsafe.Add(ref baseCollidable, i));
            }
        }

        unsafe void IntegrateVelocitiesBoundsAndInertias(int startIndex, int endIndex, float dt, ref BoundingBoxBatcher boundingBoxBatcher, int workerIndex)
        {
            ref var basePoses = ref bodies.ActiveSet.Poses[0];
            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0];
            ref var baseInertias = ref bodies.Inertias[0];
            ref var baseActivity = ref bodies.ActiveSet.Activity[0];
            ref var baseCollidable = ref bodies.ActiveSet.Collidables[0];
            for (int i = startIndex; i < endIndex; ++i)
            {
                ref var pose = ref Unsafe.Add(ref basePoses, i);
                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
                pose.Position.Validate();
                pose.Orientation.ValidateOrientation();
                velocity.Linear.Validate();
                velocity.Angular.Validate();

                UpdateSleepCandidacy(ref velocity, ref Unsafe.Add(ref baseActivity, i));

                ref var localInertia = ref Unsafe.Add(ref baseLocalInertia, i);
                ref var inertia = ref Unsafe.Add(ref baseInertias, i);
                PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out inertia.InverseInertiaTensor);
                inertia.InverseMass = localInertia.InverseMass;

                IntegrateAngularVelocity(pose, localInertia, inertia, ref velocity.Angular, dt);
                callbacks.IntegrateVelocity(i, pose, localInertia, workerIndex, ref velocity);

                boundingBoxBatcher.Add(i, pose, velocity, Unsafe.Add(ref baseCollidable, i));
            }
        }


        unsafe void IntegrateVelocities(int startIndex, int endIndex, float dt, int workerIndex)
        {
            ref var basePoses = ref bodies.ActiveSet.Poses[0];
            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
            ref var baseLocalInertia = ref bodies.ActiveSet.LocalInertias[0];
            ref var baseInertias = ref bodies.Inertias[0];
            for (int i = startIndex; i < endIndex; ++i)
            {
                ref var pose = ref Unsafe.Add(ref basePoses, i);
                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
                pose.Position.Validate();
                pose.Orientation.ValidateOrientation();
                velocity.Linear.Validate();
                velocity.Angular.Validate();

                ref var localInertia = ref Unsafe.Add(ref baseLocalInertia, i);
                ref var inertia = ref Unsafe.Add(ref baseInertias, i);
                PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out inertia.InverseInertiaTensor);
                inertia.InverseMass = localInertia.InverseMass;

                IntegrateAngularVelocity(pose, localInertia, inertia, ref velocity.Angular, dt);
                callbacks.IntegrateVelocity(i, pose, localInertia, workerIndex, ref velocity);
            }
        }

        unsafe void IntegratePoses(int startIndex, int endIndex, float dt, int workerIndex)
        {
            ref var basePoses = ref bodies.ActiveSet.Poses[0];
            ref var baseVelocities = ref bodies.ActiveSet.Velocities[0];
            for (int i = startIndex; i < endIndex; ++i)
            {
                ref var pose = ref Unsafe.Add(ref basePoses, i);
                ref var velocity = ref Unsafe.Add(ref baseVelocities, i);
                pose.Position.Validate();
                pose.Orientation.ValidateOrientation();
                velocity.Linear.Validate();
                velocity.Angular.Validate();

                PoseIntegration.Integrate(pose, velocity, dt, out pose);
            }
        }


        float cachedDt;
        int bodiesPerJob;
        IThreadDispatcher threadDispatcher;

        // 请注意,我们在这里使用的不是非常适合缓存的工作分布。
        // 这是在假设工作将足够大的前提下进行的,因此边界区域缓存未命中不会成为大问题。
        // 如果事实证明这是假的,则可以将其切换到类似于求解器的系统-
        // 预先计划每个员工的偏置区域,以允许每个员工在偷工之前冲减一个连续区域。
        int availableJobCount;
        bool TryGetJob(int bodyCount, out int start, out int exclusiveEnd)
        {
            var jobIndex = Interlocked.Decrement(ref availableJobCount);
            if (jobIndex < 0)
            {
                start = 0;
                exclusiveEnd = 0;
                return false;
            }
            start = jobIndex * bodiesPerJob;
            exclusiveEnd = start + bodiesPerJob;
            if (exclusiveEnd > bodyCount)
                exclusiveEnd = bodyCount;
            Debug.Assert(exclusiveEnd > start, "Jobs that would involve bundles beyond the body count should not be created.");
            return true;
        }

        void IntegrateBodiesAndUpdateBoundingBoxesWorker(int workerIndex)
        {
            var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt);
            var bodyCount = bodies.ActiveSet.Count;
            while (TryGetJob(bodyCount, out var start, out var exclusiveEnd))
            {
                IntegrateBodiesAndUpdateBoundingBoxes(start, exclusiveEnd, cachedDt, ref boundingBoxUpdater, workerIndex);
            }
            boundingBoxUpdater.Flush();

        }

        void PredictBoundingBoxesWorker(int workerIndex)
        {
            var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt);
            var bodyCount = bodies.ActiveSet.Count;
            while (TryGetJob(bodyCount, out var start, out var exclusiveEnd))
            {
                PredictBoundingBoxes(start, exclusiveEnd, cachedDt, ref boundingBoxUpdater, workerIndex);
            }
            boundingBoxUpdater.Flush();
        }

        void IntegrateVelocitiesBoundsAndInertiasWorker(int workerIndex)
        {
            var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, threadDispatcher.GetThreadMemoryPool(workerIndex), cachedDt);
            var bodyCount = bodies.ActiveSet.Count;
            while (TryGetJob(bodyCount, out var start, out var exclusiveEnd))
            {
                IntegrateVelocitiesBoundsAndInertias(start, exclusiveEnd, cachedDt, ref boundingBoxUpdater, workerIndex);
            }
            boundingBoxUpdater.Flush();
        }

        void IntegrateVelocitiesWorker(int workerIndex)
        {
            var bodyCount = bodies.ActiveSet.Count;
            while (TryGetJob(bodyCount, out var start, out var exclusiveEnd))
            {
                IntegrateVelocities(start, exclusiveEnd, cachedDt, workerIndex);
            }
        }

        void IntegratePosesWorker(int workerIndex)
        {
            var bodyCount = bodies.ActiveSet.Count;
            while (TryGetJob(bodyCount, out var start, out var exclusiveEnd))
            {
                IntegratePoses(start, exclusiveEnd, cachedDt, workerIndex);
            }
        }

        void PrepareForMultithreadedExecution(float dt, int workerCount)
        {
            cachedDt = dt;
            const int jobsPerWorker = 4;
            var targetJobCount = workerCount * jobsPerWorker;
            bodiesPerJob = bodies.ActiveSet.Count / targetJobCount;
            if (bodiesPerJob == 0)
                bodiesPerJob = 1;
            availableJobCount = bodies.ActiveSet.Count / bodiesPerJob;
            if (bodiesPerJob * availableJobCount < bodies.ActiveSet.Count)
                ++availableJobCount;
        }


        public void IntegrateBodiesAndUpdateBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
        {
            // 此代码路径的用户希望所有与集成相关的工作都能一次完成,因此我们需要更新惯性。
            bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count));

            var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;

            callbacks.PrepareForIntegration(dt);
            if (threadDispatcher != null)
            {
                // 虽然我们在这里确实在技术上支持多线程,但如果模拟在帧之间被踢出L3缓存,伸缩将会非常非常糟糕。
                // 在此阶段,内存负载与实际计算工作的比率非常高,因此在四核上实现1.2倍的可伸缩性是很有可能的。
                // 从好的方面看,这是一个非常短的阶段。如果运气好,以下一项或多项将成立：
                // 1)系统具有可笑的快速RAM,
                // 2)CPU支持八通道内存,只需暴力破解,
                // 3)无论应用程序在做什么,都不会逐出帧之间的整个L3缓存。

                // 请注意,这个瓶颈意味着我们以非矢量化的方式处理实体(有利于优化求解器访问的存储)这一事实不是问题。

                PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount);
                this.threadDispatcher = threadDispatcher;
                threadDispatcher.DispatchWorkers(integrateBodiesAndUpdateBoundingBoxesWorker);
                this.threadDispatcher = null;
            }
            else
            {
                var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt);
                IntegrateBodiesAndUpdateBoundingBoxes(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0);
                boundingBoxUpdater.Flush();
            }
        }

        public void PredictBoundingBoxes(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
        {
            // 这里不需要确保惯性能力;在边界框预测期间不会计算世界惯性。
            var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;

            callbacks.PrepareForIntegration(dt);
            if (threadDispatcher != null)
            {
                PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount);
                this.threadDispatcher = threadDispatcher;
                threadDispatcher.DispatchWorkers(predictBoundingBoxesWorker);
                this.threadDispatcher = null;
            }
            else
            {
                var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt);
                PredictBoundingBoxes(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0);
                boundingBoxUpdater.Flush();
            }

        }

        public void IntegrateVelocitiesBoundsAndInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
        {
            bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count));

            var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;

            callbacks.PrepareForIntegration(dt);
            if (threadDispatcher != null)
            {
                PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount);
                this.threadDispatcher = threadDispatcher;
                threadDispatcher.DispatchWorkers(integrateVelocitiesBoundsAndInertiasWorker);
                this.threadDispatcher = null;
            }
            else
            {
                var boundingBoxUpdater = new BoundingBoxBatcher(bodies, shapes, broadPhase, pool, dt);
                IntegrateVelocitiesBoundsAndInertias(0, bodies.ActiveSet.Count, dt, ref boundingBoxUpdater, 0);
                boundingBoxUpdater.Flush();
            }
        }

        public void IntegrateVelocitiesAndUpdateInertias(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
        {
            // 也期望惯性更新的子步进器使用隔离速度积分。
            bodies.EnsureInertiasCapacity(Math.Max(1, bodies.ActiveSet.Count));

            var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;

            callbacks.PrepareForIntegration(dt);
            if (threadDispatcher != null)
            {
                PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount);
                this.threadDispatcher = threadDispatcher;
                threadDispatcher.DispatchWorkers(integrateVelocitiesWorker);
                this.threadDispatcher = null;
            }
            else
            {
                IntegrateVelocities(0, bodies.ActiveSet.Count, dt, 0);
            }
        }

        public void IntegratePoses(float dt, BufferPool pool, IThreadDispatcher threadDispatcher = null)
        {
            // 这条路径与处理世界惯性计算的其他速度/边界框积分一起使用,所以我们不需要担心它。
            var workerCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;

            callbacks.PrepareForIntegration(dt);
            if (threadDispatcher != null)
            {
                PrepareForMultithreadedExecution(dt, threadDispatcher.ThreadCount);
                this.threadDispatcher = threadDispatcher;
                threadDispatcher.DispatchWorkers(integratePosesWorker);
                this.threadDispatcher = null;
            }
            else
            {
                IntegratePoses(0, bodies.ActiveSet.Count, dt, 0);
            }
        }
    }
}
